/**
 * 1.version:4.0 date:2023/3/5
 * 2.本文件仅适用于大疆电机CAN通信
 * 3.需要添加BSP和PID两个个文件
 * 4.需要开启freertos
 */

#ifndef __DJ_DRIVER_H
#define __DJ_DRIVER_H

#include "main.h"
#include "PID.h"
#include "cmsis_os.h"

#define DJ_MOTOR_CAN hcan1
#define DJ_M2006_DECRATIO 36.0f // M2006减速比
#define DJ_M3508_DECRATIO 19.0f // M3508减速比

/* 电机类型 */
enum DJ_Motor_Type
{
    M3508 = 0,
    M2006 = 1
};

/* 电机ID地址 */
enum CAN_Motor_ID
{

    DJ_H_ID = 0x1FF,
    DJ_L_ID = 0x200,
    DJ_M1_ID = 0x201,
    DJ_M2_ID = 0x202,
    DJ_M3_ID = 0x203,
    DJ_M4_ID = 0x204,
    DJ_M5_ID = 0x205,
    DJ_M6_ID = 0x206,
    DJ_M7_ID = 0x207,
    DJ_M8_ID = 0x208,
};

/* 电机反馈参数 */
typedef struct
{
    int16_t setAngle;   // 设置角度
    int16_t setSpeed;   // 设置速度
    int16_t setCurrent; // 设置电流

    uint16_t angle;      // CAN读取的角度
    int16_t speed;       // CAN读取的速度
    int16_t current;     // CAN读取的电流
    uint16_t last_angle; // 上一次读取的角度

    uint16_t ID;           // 电机的ID
    uint8_t motor_type;    // 电机的类型
    float decratio;        // 电机的减速比
    uint16_t offset_angle; // 电机初始偏移角度
    float total_angle;     // 电机总角度
    int32_t round_count;   // 电机圈数

    PID_t PID_Speed;        // 电机速度环PID
    PID_t PID_Angle;        // 电机角度环PID
    PID_t PID_SpeedOfAngle; // 电机位置控制的速度环PID
} DJ_Motor_t;

void DJ_TaskInit(void);
void DJ_Init(DJ_Motor_t *motor, uint8_t Motor_ID, uint8_t Motor_Type);
void DJ_SetAngle(DJ_Motor_t *motor, int16_t angle);
void DJ_SetSpeed(DJ_Motor_t *motor, int16_t speed);
void DJ_ClearAngle(DJ_Motor_t *motor);

#endif
